/*
** Shp.c  -   This file contains routine methods of element shape
**            super class.
**
** --------------------------------------------------------------------------
**
** Entry Points:
**
** --------------------------------------------------------------------------
**
** void shp_init( void )
**
**      Initializes the  globally shared  "shape_class"  variable. This
**      variable  holds the pointers to the necessary functions  which
**      implement the element shape class methods.
**
** --------------------------------------------------------------------------
**
** void shp_free ( void )
**
**      Releases memory used by super-classe shap.
**      Currently it does nothing.
**
** --------------------------------------------------------------------------
**
** void shp_freeshape( Telement_3d *shp )
**
**      shp        -  given element shape                      (  in )
**
**      This routine releases the memory used by a generic element shape.
**
** --------------------------------------------------------------------------
**
** void shp_gausstonode( Telement_3d *shp, double *gauss_resp, double *node_resp )
**
**      shp        -  given element shape                      (  in )
**      gauss_resp -  given vector of Gauss point responses    (  in )
**      node_resp  -  returned vector of nodal responses       ( out )
**
**      This routine extrapolates from Gauss points' responses to nodal 
**      responses for the given element shape.
**
** --------------------------------------------------------------------------
**
** void shp_nodetovertex( Telement_3d *shp, double *node_resp, double *vert_resp )
**
**      shp        -  given element shape                      (  in )
**      node_resp  -  given nodal point response vector        (  in )
**      vert_resp  -  returned vertex response vector          ( out )
**
**      This is the default function for translation from fem node 
**      results to vertex results.  It assumes that all mid-edge nodes 
**      exist and that there is no collapsed-edge vertex.  It simply
**      copies from the given node result vertor to the vertex result
**      vector.
**
** --------------------------------------------------------------------------
**
** int  shp_isinterface( Telement_3d *shp )
**
**      shp        -  given element shape                      (  in )
**
**      This is the default function for inquiring for interface 
**      (discontinuous) finite element shape.  It returns a false 
**      status indicating that the the default shape is continuous.
**
** --------------------------------------------------------------------------
**
** Created:  29-Jul-96   -   Luiz Fernando Martha
**      Stolen from Shape class of Femoop and modified.
**
** --------------------------------------------------------------------------
**
*/

#define  _SHP_CPP

/*
**  -------------------------------------------------------------------------
**  Global variables and symbols:
*/

#include <stdlib.h>
#include <stdio.h>
#include <string.h>

#include "shp.h"

/* Here follow the init methods for the shape-class mechanism */

extern void  InitTETR4     ( void );  
extern void  InitTETR10    ( void );  
extern void  InitWEDGE6    ( void );  
extern void  InitWEDGE15   ( void );  
extern void  InitBRICK8    ( void );
extern void  InitBRICK20   ( void );  

/*
**  -------------------------------------------------------------------------
**  Local functions:
*/
static double **ProfileAlloc( int  n, int *profile );
static void ProfileFree( double **m, int nr, int *profile );
#if 0
static int CroutSolver( int ii, double **a, double *b, int n, int *c, int *f );
#else
static int CroutSolver( int ii, double **a, double *b, int n, int *c );
#endif

/* ============================ ProfileAlloc ============================= */

static double **ProfileAlloc( int  n, int *profile )
{
 int    i;
 double **address;
 double *v;

 address = (double **)calloc( n, sizeof(double *) );

 if( !address ) {
  printf( "Allocation failure in ProfileAlloc !!!\n" );
  exit( 1 );
 }

 for( i=0; i<n; i++ )
 {
  v = (double *)calloc( i-profile[i]+1, sizeof(double) );
  if( !v ) {
   printf( "Allocation failure in ProfileAlloc !!!\n" );
   exit( 1 );
  }
  address[i] = v - profile[i];
 }

 return (address);
}

/* ============================ ProfileFree ============================== */

static void ProfileFree( double **m, int nr, int *profile )
{
 int    i;

 for( i=0; i<nr; i++ )
  free( m[i]+profile[i] );
 free( m );
}

/* =============================== CroutSolver =========================== */
/*
** int CroutSolver ( int      ii,
**                   double   **a,
**                   double   *b,
**                   int      n,
**                   int      *c,
**                   int      *f )
**
**      ii     -   flag that controls type of solution, i.e.:
**                 ii = 1 - complete solution
**                 ii = 2 - decomposition of coefficient matrix
**                 ii = 3 - back substitution
**      a      -   coefficient matrix  ( symmetrical matrix )      (  in )
**      b      -   independent terms / system solution          ( in/out )
**      n      -   system order                                    (  in )
**      c      -   a vector which indicates for each  column  of  [a]  the
**                 lowest row which contains a non-zero element.   (  in )
**      f      -   flag that indicates if solution exists          ( out )
**
**      Developed by:   Luiz Henrique de Fiqueiredo.
**
**      Solves a system of linear equations: [a].{x} = {b}
**      where [a] is stored in a vector ( profile ).
*/

#if 0
static int CroutSolver( int ii, double **a, double *b, int n, int *c, int *f )
#else
static int CroutSolver( int ii, double **a, double *b, int n, int *c )
#endif
{
 int  i, j, k;

#if 0
 *f = 1;
#endif

 if( ii != 3 )
 {
/* Reduction of matrix [a].
 */
  for (j=1;j<n;j++)
  {
   for (i=c[j]+1;i<j;i++)
    for (k=( c[i]>c[j] ? c[i] : c[j] );k<i;k++)
     a[j][i] -= a[i][k] * a[j][k];
   for (k=c[j]; k<j; k++) {
#if 0
    if ( fabs(a[k][k]) < TOL )
    {
     *f = 0;
     return( 0 );
    }
#endif
    a[j][j] -= a[j][k] / a[k][k] * a[j][k];
    a[j][k] /= a[k][k];
   }
  }
 }

 if( ii == 2 ) return( 1 );

/* Reduction of vector {b}.
 */
 for (i=1;i<n;i++)
 {
  for (k=c[i];k<i;k++)
   b[i] -= a[i][k] * b[k];
 }
 for (i=0;i<n;i++)
 {
  b[i] /= a[i][i];
 }

/* Back-substitution.
 */
 for (i=n-1;i>0;i--)
 {
  for (k=c[i]; k<i; k++)
   b[k] -= a[i][k] * b[i];
 }

 return( 1 );
}

/*
**  -------------------------------------------------------------------------
**  Public Entry Points:
*/

/* ============================= shp_init ================================ */

void shp_init ( void )
{
 int i;

 shape_class[NF_TETR4].init        = InitTETR4;   
 shape_class[NF_TETR10].init       = InitTETR10;   
 //shape_class[NF_WEDGE6].init       = InitWEDGE6;   
 //shape_class[NF_WEDGE15].init      = InitWEDGE15;   
 shape_class[NF_BRICK8].init       = InitBRICK8;
 shape_class[NF_BRICK20].init      = InitBRICK20;   

 for( i=0; i<NumShapeTypes; i++ )
 {
  ShpInit( i );
 }
}

/* ============================= shp_free =============================== */

void shp_free( void )
{
}

/* ========================== shp_freeshape ============================= */

void shp_freeshape( Telement_3d *shp )
{

}

/* =========================== shp_gausstonode ========================== */

void shp_gausstonode (Telement_3d *shp, double *gauss_resp, double *node_resp)
{
#if 1
 static  int        curr_shapetype = (int) NF_UNDEF;
 static  int        curr_ordlabel = -1;
 static  int        num_mapnodes = 0;
 static  int        num_gausspts = 0;
 static  double     tr_matrix[20][27];
 static  double     node_matrix[20][4];
 static  double     gauss_matrix[27][4];
 static  NatCoord   node_rst[20];
 static  NatCoord   gauss_rst;
 int                dim_sum_matrix;
 int                *sum_profile;
 double             **sum_matrix;
 double             gau_vec[4];
 int                i, j, k;

  // Check to see if current element shape and Gauss integration order 
  // are the same as the last ones.  In this case, just extrapolate 
  // based on previous computed extrapolation matrix.
  if( (ShpType( shp ) == curr_shapetype))
  {
    for( i=0; i<num_mapnodes; i++ )
    {
      node_resp[i] = 0.0;
      for( j=0; j<num_gausspts; j++ )
        node_resp[i] += tr_matrix[i][j] * gauss_resp[j];
    }
    return;
  }

  // Update control variables for next call.
  curr_shapetype = ShpType( shp );

  // Get number of shape nodes and number of Gauss points.
  num_mapnodes = ShpNumMapNodes( shp );
  num_gausspts = ShpNumGaussPoint(shp);

  // Check special case of one point integration order.
  // In this case, TRMatrix is a column vector with 1's.
  if( num_gausspts == 1 )
  {
    for( i=0; i<num_mapnodes; i++ )
    {
      tr_matrix[i][0] = 1.0;
      node_resp[i] = gauss_resp[0];
    }
    return;
  }

  // Get memory for the profile vector of summation matrix and initialize it.
  dim_sum_matrix = 1 + ShpDim( shp );
  sum_profile = (int *)calloc( dim_sum_matrix, sizeof(int) );
  for( i=0; i<dim_sum_matrix; i++ ) 
    sum_profile[i] = 0;

  // Get memory for summation matrix ([P]) and initialize it.
  sum_matrix = ProfileAlloc( dim_sum_matrix, sum_profile );
  for( j=0; j<dim_sum_matrix; j++ )
    for( k=0; k<=j; k++ )
      sum_matrix[j][k] = 0.0;


  // Mount [P] (summation) and [Q] (extended Gauss natural coordinates) matrices.
  for (i = 0; i < num_gausspts; i++)
  {
    // Get Gauss points' natural coordinates.
    ShpGaussNatCoord (shp, i, &gauss_rst);

    // Store current Gauss point natural coordinates as an expanded vector.
    gau_vec[0] = 1.0;
    gau_vec[1] = gauss_rst.r;
    gau_vec[2] = gauss_rst.s;
    gau_vec[3] = gauss_rst.t;

    // Assemble current Gauss contribution into summation and Gauss matrices.
    for (j = 0; j <= ShpDim (shp); j++)
    {
      gauss_matrix[i][j] = gau_vec[j];
      for (k = 0; k <= j; k++)
        sum_matrix[j][k] += gau_vec[j] * gau_vec[k];
    }
  }

  // Decompose summation matrix.
  CroutSolver( 2, sum_matrix, 0L, dim_sum_matrix, sum_profile );

  // Compute [S] = [P]^-1 * [Q] and store it in [Q].
  for( i=0; i<num_gausspts; i++ )
  {
    CroutSolver(3, sum_matrix, &gauss_matrix[i][0], dim_sum_matrix, sum_profile);
  }

  // Get element shape node natural coordinates.
  ShpNodalNatCoord ( shp, node_rst );

  // Mount extended node natural coordinate matrix.
  for( i=0; i<num_mapnodes; i++ )
  {
    node_matrix[i][0] = 1.0;
    node_matrix[i][1] = node_rst[i].r;
    node_matrix[i][2] = node_rst[i].s;
    node_matrix[i][3] = node_rst[i].t;
  }

  // Compute [TR] (extrapolation) matrix and extrapolate from gauss pts to nodes.
  for( i=0; i<num_mapnodes; i++ )
  {
    node_resp[i] = 0.0;
    for( j=0; j<num_gausspts; j++ )
    {
      tr_matrix[i][j] = 0.0;
      for( k=0; k<dim_sum_matrix; k++ )
        tr_matrix[i][j] += node_matrix[i][k] * gauss_matrix[j][k];
      node_resp[i] += tr_matrix[i][j] * gauss_resp[j];
    }
  }

  /* Free auxiliary memory.
  */
  ProfileFree( sum_matrix, dim_sum_matrix, sum_profile );
  free( sum_profile );
  #endif
}

/* ========================== shp_nodetovertex ========================== */

void shp_nodetovertex( Telement_3d *shp, double *node_resp, double *vert_resp )
{
 int     i;

 for( i=0; i<ShpNumMapNodes( shp ); i++ )
  vert_resp[i] = node_resp[i];
}

/* =========================== shp_isinterface ========================== */

int shp_isinterface( Telement_3d *shp )
{
 ignore( shp );
 return( 0 );
}

/* =========================== ShpMathInver3x3 ========================== */
void ShpMathInver3x3 (double a[3][3], double b[3][3], double *det)
{
  int i,j;
 
  /* Clear inverse matrix.
  */
   for (i = 0;i < 3; i++)
     for (j = 0;j < 3; j++) b[i][j] = 0.0;
 
  /* Compute matrix determinant.
  */
  (*det) = a[0][0] * ( a[1][1] * a[2][2] - a[1][2] * a[2][1] ) -
           a[0][1] * ( a[1][0] * a[2][2] - a[1][2] * a[2][0] ) +
           a[0][2] * ( a[1][0] * a[2][1] - a[1][1] * a[2][0] );
 
  if ( (*det) > 0.0  )
  {
    b[0][0] =  ( a[1][1] * a[2][2] - a[1][2] * a[2][1] ) / (*det);
    b[0][1] = -( a[0][1] * a[2][2] - a[0][2] * a[2][1] ) / (*det);
    b[0][2] =  ( a[0][1] * a[1][2] - a[0][2] * a[1][1] ) / (*det);
    b[1][0] = -( a[1][0] * a[2][2] - a[1][2] * a[2][0] ) / (*det);
    b[1][1] =  ( a[0][0] * a[2][2] - a[0][2] * a[2][0] ) / (*det);
    b[1][2] = -( a[0][0] * a[1][2] - a[0][2] * a[1][0] ) / (*det);
    b[2][0] =  ( a[1][0] * a[2][1] - a[1][1] * a[2][0] ) / (*det);
    b[2][1] = -( a[0][0] * a[2][1] - a[0][1] * a[2][0] ) / (*det);
    b[2][2] =  ( a[0][0] * a[1][1] - a[0][1] * a[1][0] ) / (*det);
  }
  else
  {
    printf( "\n\n Singular Matrix in Function MathInver3x3 !! \n" );
    exit( 1 );
  }

}

/* ============================== shp_jacobian =========================== */

void shp_jacobian ( Telement_3d *shp, Tnode_data *coord, DerivNat *deriv_rst, 
                    double jac[3][3], double invjac[3][3], double *det)
{
  int i;
  memset (jac, 0, 9 * sizeof (double));
  memset (invjac, 0, 9 * sizeof (double));
 
  for (i = 0; i < shp->n; i++)
  {
    jac[0][0] += deriv_rst[i].r * coord[shp->inc[i]].x;
    jac[0][1] += deriv_rst[i].r * coord[shp->inc[i]].y;
    jac[0][2] += deriv_rst[i].r * coord[shp->inc[i]].z;

    jac[1][0] += deriv_rst[i].s * coord[shp->inc[i]].x;
    jac[1][1] += deriv_rst[i].s * coord[shp->inc[i]].y;
    jac[1][2] += deriv_rst[i].s * coord[shp->inc[i]].z;

    jac[2][0] += deriv_rst[i].t * coord[shp->inc[i]].x;
    jac[2][1] += deriv_rst[i].t * coord[shp->inc[i]].y;
    jac[2][2] += deriv_rst[i].t * coord[shp->inc[i]].z;
  }

  ShpMathInver3x3 (jac, invjac, det);
}

/* ============================== shp_derivXYZ =========================== */

void shp_derivXYZ ( Telement_3d *shp, double invjac[3][3], 
                    DerivNat *derivshp_rst, DerivCart *deriv_xyz )
{
  int i;

  for (i = 0; i < shp->n; i++)
  {
    deriv_xyz[i].x = invjac[0][0] * derivshp_rst[i].r +
                     invjac[0][1] * derivshp_rst[i].s +
                     invjac[0][2] * derivshp_rst[i].t;

    deriv_xyz[i].y = invjac[1][0] * derivshp_rst[i].r +
                     invjac[1][1] * derivshp_rst[i].s +
                     invjac[1][2] * derivshp_rst[i].t;

    deriv_xyz[i].z = invjac[2][0] * derivshp_rst[i].r +
                     invjac[2][1] * derivshp_rst[i].s +
                     invjac[2][2] * derivshp_rst[i].t;
  }
}


/* ============================== shp_BMatrix =========================== */
void shp_BMatrix (Telement_3d *shp, DerivCart *deriv_xyz,
                  double **b_matrix)
{
  int i;
  for (i = 0; i < shp->n; i++)
  {
    b_matrix[0][3*i  ] = deriv_xyz[i].x;
    b_matrix[0][3*i+1] = 0.0;
    b_matrix[0][3*i+2] = 0.0;
 
    b_matrix[1][3*i  ] = 0.0;
    b_matrix[1][3*i+1] = deriv_xyz[i].y;
    b_matrix[1][3*i+2] = 0.0;
 
    b_matrix[2][3*i  ] = 0.0;
    b_matrix[2][3*i+1] = 0.0;
    b_matrix[2][3*i+2] = deriv_xyz[i].z;
 
    b_matrix[3][3*i  ] = deriv_xyz[i].y;
    b_matrix[3][3*i+1] = deriv_xyz[i].x;
    b_matrix[3][3*i+2] = 0.0;
 
    b_matrix[4][3*i  ] = deriv_xyz[i].z;
    b_matrix[4][3*i+1] = 0.0;
    b_matrix[4][3*i+2] = deriv_xyz[i].x;
 
    b_matrix[5][3*i  ] = 0.0;
    b_matrix[5][3*i+1] = deriv_xyz[i].z;
    b_matrix[5][3*i+2] = deriv_xyz[i].y;
  }
}

/* =========================== NodalCoordTETR10 ============================= */
void shp_NodalCoord ( Telement_3d *shp, Tnode_data *node_coords, 
                      Tnode_data *coord )
{
   int      i;
   for (i = 0; i < shp->n; ++i)
   {
      coord[i].x = node_coords[shp->inc[i]-1].x;
      coord[i].y = node_coords[shp->inc[i]-1].y;
      coord[i].z = node_coords[shp->inc[i]-1].z;
   }
}
